#include <ros/ros.h>
#include <asctec_hl_comm/MotorSpeed.h>

#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>


int count = 0;
int s1 = 11;


geometry_msgs::Twist SysID;
ros::Publisher SysID_pub;


void MotorFeedback_callback(const asctec_hl_comm::MotorSpeed & msg)
{

 s1 = msg.motor_speed[0];

SysID.angular.x = msg.motor_speed[0];
SysID.angular.y = msg.motor_speed[1];
SysID.angular.z = msg.motor_speed[2];


 ROS_INFO("Actual_MOTOR_SPEED: %d %d %d %d",msg.motor_speed[0],msg.motor_speed[1],msg.motor_speed[2],msg.motor_speed[3]);

}


int main(int argc, char** argv)
{

 // int count = 0;

  ros::init(argc, argv, "MotorSpeedClient");
  ros::NodeHandle nh("~");
  asctec_hl_comm::MotorSpeed mtr; 
  asctec_hl_comm::MotorSpeed mtr1;  


  mtr1.motor_speed[0] = 0; 
  mtr1.motor_speed[1] = 0;
  mtr1.motor_speed[2] = 0;
  mtr1.motor_speed[3] = 0; 
  
  ros::Publisher motorSpeedPublisher = nh.advertise<asctec_hl_comm::MotorSpeed>("/fcu/motor_speed_ctrl",1);
 
  SysID_pub = nh.advertise<geometry_msgs::Twist>("/PelicanMotorSysID",1);//Publisher is used to publish input and output data used for the system ID of the motors 

  ros::Subscriber MotorFeedback = nh.subscribe("/fcu/motor_speed",1, MotorFeedback_callback);


  ros::Duration timeout(15);

  if (argc == 5)
  {
	for (int i= 0; i < 4; i ++){
		mtr.motor_speed[i] = atoi(argv[i+1]);
		if (mtr.motor_speed[i] > 200 || mtr.motor_speed[i] < 0){
			ROS_WARN("Improper motor speeds");
			return -1;
		}
		
	}
	/*printf("got %hu %hu\n", atoi(argv[1]), atoi(argv[2]));
	sprintf(argv[1], "%hu", &(mtr.motor_speed[0]));
	sprintf(argv[2], "%hu", &(mtr.motor_speed[1]));
	sprintf(argv[3], "%hu", &(mtr.motor_speed[2]));
	sprintf(argv[4], "%hu", &(mtr.motor_speed[3]));*/
  }
  else
  {
    std::cout << "Wrong number of arguments! \nusage:" << "\tmotorSpeedClient speed1 speed2 speed3 speed4 i\n"<<std::endl;
    return -1;
  }

  ros::Rate loop(1000);
  while(ros::ok()){

  // want to stop the motor for few seconds and then given some step input.
  count = count + 1;  
  if (count < 10000)
  {
  mtr1.header.stamp = ros::Time::now();
  motorSpeedPublisher.publish(mtr1);
  SysID.linear.x = mtr1.motor_speed[0];
  }
  else
  {
  mtr.header.stamp = ros::Time::now();
  motorSpeedPublisher.publish(mtr);
  
  SysID.linear.x = mtr.motor_speed[0];
  SysID.linear.y = mtr.motor_speed[1];
  SysID.linear.z = mtr.motor_speed[2]; 
  }
  
  SysID_pub.publish(SysID);
  
	loop.sleep();
	ros::spinOnce();
  }

  return 0;
}
